package shared;

import javax.media.opengl.GL2;

// The GLFrame (OrthonormalFrame) class.  Possibly the most useful little piece of 3D graphics
// code for OpenGL immersive environments.
// Richard S. Wright Jr.
public class GLFrame
{
	protected float vOrigin[] = new float[3]; // Where am I?
	protected float vForward[] = new float[3]; // Where am I going?
	protected float vUp[] = new float[3]; // Which way is up?
	
	// Default position and orientation. At the origin, looking
	// down the positive Z axis (right handed coordinate system).
	public GLFrame()
	{
		// At origin
		vOrigin[0] = 0.0f; vOrigin[1] = 0.0f; vOrigin[2] = 0.0f;
		
		// Up is up (+Y)
		vUp[0] = 0.0f; vUp[1] = 1.0f; vUp[2] = 0.0f;
		
		// Forward is -Z (default OpenGL)
		vForward[0] = 0.0f; vForward[1] = 0.0f; vForward[2] = -1.0f;
	}
	
	/////////////////////////////////////////////////////////////
	// Set Location
	public void setOrigin(float vPoint[])
	{
		M3D.copyVector3(vOrigin, vPoint);
	}
	
	public void setOrigin(float x, float y, float z)
	{
		vOrigin[0] = x; vOrigin[1] = y; vOrigin[2] = z;
	}
	
	public void getOrigin(float vPoint[])
	{
		M3D.copyVector3(vPoint, vOrigin);
	}
	
	public float getOriginX()
	{
		return vOrigin[0];
	}
	
	public float getOriginY()
	{
		return vOrigin[1];
	}
	
	public float getOriginZ()
	{
		return vOrigin[2];
	}
	
	/////////////////////////////////////////////////////////////
	// Set Forward Direction
	public void setForwardVector(float vDirection[])
	{
		M3D.copyVector3(vForward, vDirection);
	}
	
	public void setForwardVector(float x, float y, float z)
	{
		vForward[0] = x; vForward[1] = y; vForward[2] = z;
	}
	
	public void getForwardVector(float vVector[])
	{
		M3D.copyVector3(vVector, vForward);
	}
	
	/////////////////////////////////////////////////////////////
	// Set Up Direction
	public void setUpVector(float vDirection[])
	{
		M3D.copyVector3(vUp, vDirection);
	}
	
	public void setUpVector(float x, float y, float z)
	{
		vUp[0] = x; vUp[1] = y; vUp[2] = z;
	}
	
	public void getUpVector(float vVector[])
	{
		M3D.copyVector3(vVector, vUp);
	}
	
	/////////////////////////////////////////////////////////////
	// Get Axes
	public void getZAxis(float vVector[])
	{
		getForwardVector(vVector);
	}
	
	public void getYAxis(float vVector[])
	{
		getUpVector(vVector);
	}
	
	public void getXAxis(float vVector[])
	{
		M3D.crossProduct(vVector, vUp, vForward);
	}
	
	/////////////////////////////////////////////////////////////
	// Translate along orthonormal axis... world or local
	public void translateWorld(float x, float y, float z)
	{
		vOrigin[0] += x; vOrigin[1] += y; vOrigin[2] += z;
	}
	
	public void translateLocal(float x, float y, float z)
	{
		moveForward(z); moveUp(y); moveRight(x);
	}
	
	/////////////////////////////////////////////////////////////
	// Move Forward (along Z axis)
	public void moveForward(float fDelta)
	{
		// Move along direction of front direction
		vOrigin[0] += vForward[0] * fDelta;
		vOrigin[1] += vForward[1] * fDelta;
		vOrigin[2] += vForward[2] * fDelta;
	}
	
	// Move along Y axis
	public void moveUp(float fDelta)
	{
		// Move along direction of up direction
		vOrigin[0] += vUp[0] * fDelta;
		vOrigin[1] += vUp[1] * fDelta;
		vOrigin[2] += vUp[2] * fDelta;
	}
	
	// Move along X axis
	public void moveRight(float fDelta)
	{
		// Move along direction of right vector
		float vCross[] = new float[3];
		M3D.crossProduct(vCross, vUp, vForward);
		
		vOrigin[0] += vCross[0] * fDelta;
		vOrigin[1] += vCross[1] * fDelta;
		vOrigin[2] += vCross[2] * fDelta;
	}
	
	///////////////////////////////////////////////////////////////////////
	// Just assemble the matrix
	public void getMatrix(float matrix[])
	{
		getMatrix(matrix, false);
	}
	
	public void getMatrix(float matrix[], boolean bRotationOnly)
	{
		// Calculate the right side (x) vector, drop it right into the matrix
		float vXAxis[] = new float[3];
		M3D.crossProduct(vXAxis, vUp, vForward);
		
		// Set matrix column does not fill in the fourth value...
		M3D.setMatrixColumn44(matrix, vXAxis, 0);
		matrix[3] = 0.0f;
		
		// Y Column
		M3D.setMatrixColumn44(matrix, vUp, 1);
		matrix[7] = 0.0f;
		
		// Z Column
		M3D.setMatrixColumn44(matrix, vForward, 2);
		matrix[11] = 0.0f;
		
		// Translation (already done)
		if(bRotationOnly)
		{
			matrix[12] = 0.0f;
			matrix[13] = 0.0f;
			matrix[14] = 0.0f;
		}
		else
			M3D.setMatrixColumn44(matrix, vOrigin, 3);
		
		matrix[15] = 1.0f;
	}

	/////////////////////////////////////////////////////////////
	// Get a 4x4 transformation matrix that describes the ccamera
	// orientation.
	public void getCameraOrientation(float m[])
	{
		float x[] = new float[3];
		float z[] = new float[3];
		
		// Make rotation matrix
		// Z vector is reversed
		z[0] = -vForward[0];
		z[1] = -vForward[1];
		z[2] = -vForward[2];
		
		// X vector = Y cross Z
		M3D.crossProduct(x, vUp, z);
		
		// Matrix has no translation information and is
		// transposed.... (rows instead of columns)
		m[0 * 4 + 0] = x[0];
		m[1 * 4 + 0] = x[1];
		m[2 * 4 + 0] = x[2];
		m[3 * 4 + 0] = 0.0f;
		m[0 * 4 + 1] = vUp[0];
		m[1 * 4 + 1] = vUp[1];
		m[2 * 4 + 1] = vUp[2];
		m[3 * 4 + 1] = 0.0f;
		m[0 * 4 + 2] = z[0];
		m[1 * 4 + 2] = z[1];
		m[2 * 4 + 2] = z[2];
		m[3 * 4 + 2] = 0.0f;
		m[0 * 4 + 3] = 0.0f;
		m[1 * 4 + 3] = 0.0f;
		m[2 * 4 + 3] = 0.0f;
		m[3 * 4 + 3] = 1.0f;
	}
	
	/////////////////////////////////////////////////////////////
	// Perform viewing or modeling transformations
	// Position as the camera (for viewing). Apply this transformation
	// first as your viewing transformation
	// The default implementation of gluLookAt can be considerably sped up
	// since it uses doubles for everything... then again profile before you
	// tune... ;-) You might get a boost form page fault reduction too... if
	// no other glu routines are used...
	// This will get called once per frame.... go ahead and inline
	public void applyCameraTransform(GL2 gl2)
	{
		applyCameraTransform(gl2, false);
	}
	
	public void applyCameraTransform(GL2 gl2, boolean bRotOnly)
	{	
		float m[] = new float[16];
		
		getCameraOrientation(m);
		
		// Camera Transform
		gl2.glMultMatrixf(m, 0);
		
		// If Rotation only, then do not do the translation
		if(!bRotOnly)
			gl2.glTranslatef(-vOrigin[0], -vOrigin[1], -vOrigin[2]);
	}
	
	// Position as an object in the scene. This places and orients a
	// coordinate frame for other objects (besides the camera)
	// There is ample room for optimization here... 
	// This is going to be called alot... don't inline
	// Add flag to perform actor rotation only and not the translation
	public void applyActorTransform(GL2 gl2)
	{
		applyActorTransform(gl2, false);
	}
	
	public void applyActorTransform(GL2 gl2, boolean bRotationOnly)
	{
		float rotMat[] = new float[16];
		
		getMatrix(rotMat, bRotationOnly);
		
		// Apply rotation to the current matrix
		gl2.glMultMatrixf(rotMat, 0);
	}
	
	// Rotate around local X Axes - Note all rotations are in radians
	public void rotateLocalX(float fAngle)
	{
		float rotMat[] = new float[16];
		float vCross[] = new float[3];
		M3D.crossProduct(vCross, vUp, vForward);
		M3D.rotationMatrix44(rotMat, fAngle, vCross[0], vCross[1], vCross[2]);
		
		float newVect[] = new float[3];
		// Inline 3x3 matrix multiply for rotation only
		newVect[0] = rotMat[0] * vForward[0] + rotMat[4] * vForward[1] + rotMat[8] * vForward[2];
		newVect[1] = rotMat[1] * vForward[0] + rotMat[5] + vForward[1] + rotMat[9] * vForward[2];
		newVect[2] = rotMat[2] * vForward[0] + rotMat[6] + vForward[1] + rotMat[10] * vForward[2];
		M3D.copyVector3(vForward, newVect);
		
		// Update pointing up vector
		newVect[0] = rotMat[0] * vUp[0] + rotMat[4] * vUp[1] + rotMat[8] * vUp[2];
		newVect[1] = rotMat[1] * vUp[0] + rotMat[5] * vUp[1] + rotMat[9] * vUp[2];
		newVect[2] = rotMat[2] * vUp[0] + rotMat[6] * vUp[1] + rotMat[10] * vUp[2];
		M3D.copyVector3(vUp, newVect);
	}
	
	// Rotate around local Y
	public void rotateLocalY(float fAngle)
	{
		float rotMat[] = new float[16];
		
		// Just Rotate around the up vector
		// Create a rotation matrix around my Up (Y) vector
		M3D.rotationMatrix44(rotMat, fAngle, vUp[0], vUp[1], vUp[2]);
		
		float newVect[] = new float[3];
		
		// Rotate forward pointing vector (inlined 3x3 transform)
		newVect[0] = rotMat[0] * vForward[0] + rotMat[4] * vForward[1] + rotMat[8] * vForward[2];
		newVect[1] = rotMat[1] * vForward[0] + rotMat[5] * vForward[1] + rotMat[9] * vForward[2];
		newVect[2] = rotMat[2] * vForward[0] + rotMat[6] * vForward[1] + rotMat[10] * vForward[2];
		M3D.copyVector3(vForward, newVect);
	}
	
	// Rotate around local Z
	public void rotateLocalZ(float fAngle)
	{
		float rotMat[] = new float[16];
		
		// Only the up vector needs to be rotated
		M3D.rotationMatrix44(rotMat, fAngle, vForward[0], vForward[1], vForward[2]);
		
		float newVect[] = new float[3];
		newVect[0] = rotMat[0] * vUp[0] + rotMat[4] * vUp[1] + rotMat[8] * vUp[2];
		newVect[1] = rotMat[1] * vUp[0] + rotMat[5] * vUp[1] + rotMat[9] * vUp[2];
		newVect[2] = rotMat[2] * vUp[0] + rotMat[6] * vUp[1] + rotMat[10] * vUp[2];
		M3D.copyVector3(vUp, newVect);
	}
	
	// Reset axes to make sure they are orthonormal.  This should be called on occasion
	// if the matrix is long-lived and frequently transformed.
	public void normalize()
	{
		float vCross[] = new float[3];
		
		// Calculate cross product of up and forward vectors
		M3D.crossProduct(vCross, vUp, vForward);
		
		// Use result to recalculate forward vector
		M3D.crossProduct(vForward, vCross, vUp);
		
		// Also check for unit length...
		M3D.normalizeVector(vUp);
		M3D.normalizeVector(vForward);
	}
	
	// Rotate in world coordinates...
	public void rotateWorld(float fAngle, float x, float y, float z)
	{
		float rotMat[] = new float[16];
		
		// Create the Rotation matrix
		M3D.rotationMatrix44(rotMat, fAngle, x, y, z);
		
		float newVect[] = new float[3];
		
		// Transform the up axis (inlined 3x3 rotation)
		newVect[0] = rotMat[0] * vUp[0] + rotMat[4] * vUp[1] + rotMat[8] * vUp[2];
		newVect[1] = rotMat[1] * vUp[0] + rotMat[5] * vUp[1] + rotMat[9] * vUp[2];
		newVect[2] = rotMat[2] * vUp[0] + rotMat[6] * vUp[1] + rotMat[10] * vUp[2];
		M3D.copyVector3(vUp, newVect);
		
		// Transform the forward axis
		newVect[0] = rotMat[0] * vForward[0] + rotMat[4] * vForward[1] + rotMat[8] * vForward[2];
		newVect[1] = rotMat[1] * vForward[0] + rotMat[5] * vForward[1] + rotMat[9] * vForward[2];
		newVect[2] = rotMat[2] * vForward[0] + rotMat[6] * vForward[1] + rotMat[10] * vForward[2];
		M3D.copyVector3(vForward, newVect);
	}
	
	// Rotate around a local axis
	public void rotateLocal(float fAngle, float x, float y, float z)
	{
		float vWorldVect[] = new float[3];
		float vLocalVect[] = new float[3];
		M3D.loadVector3(vLocalVect, x, y, z);
		
		localToWorld(vLocalVect, vWorldVect);
		rotateWorld(fAngle, vWorldVect[0], vWorldVect[1], vWorldVect[2]);
	}
	
	// Convert Coordinate Systems
    // This is pretty much, do the transformation represented by the rotation
    // and position on the point
	// Is it better to stick to the convention that the destination always comes
	// first, or use the conventions that "sounds" like the function...
	public void localToWorld(float vLocal[], float vWorld[])
	{
		// Create the rotation matrix based on the vectors
		float rotMat[] = new float[16];
		
		getMatrix(rotMat, true);
		
		// Do the rotation (inline it, and remove 4th column...)
		vWorld[0] = rotMat[0] * vLocal[0] + rotMat[4] * vLocal[1] + rotMat[8] * vLocal[2];
		vWorld[1] = rotMat[1] * vLocal[0] + rotMat[5] * vLocal[1] + rotMat[9] * vLocal[2];
		vWorld[2] = rotMat[2] * vLocal[0] + rotMat[6] * vLocal[1] + rotMat[10] * vLocal[2];
		
		// Translate the point
		vWorld[0] += vOrigin[0];
		vWorld[1] += vOrigin[1];
		vWorld[2] += vOrigin[2];
	}

	// Change world coordinates into "local" coordinates
	public void worldToLocal(float vWorld[], float vLocal[])
	{
		////////////////////////////////////////////////
        // Translate the origin
		float vNewWorld[] = new float[3];
		vNewWorld[0] = vWorld[0] - vOrigin[0];
		vNewWorld[1] = vWorld[1] - vOrigin[1];
		vNewWorld[2] = vWorld[2] - vOrigin[2];
		
		// Create the rotation matrix based on the vectors
		float rotMat[] = new float[16];
		float invMat[] = new float[16];
		getMatrix(rotMat, true);
		
		// Do the rotation based on inverted matrix
		M3D.invertMatrix44(invMat, rotMat);
		
		vLocal[0] = invMat[0] * vNewWorld[0] + invMat[4] * vNewWorld[1] + invMat[8] * vNewWorld[2];
		vLocal[1] = invMat[1] * vNewWorld[0] + invMat[5] * vNewWorld[1] + invMat[9] * vNewWorld[2];
		vLocal[2] = invMat[2] * vNewWorld[0] + invMat[6] * vNewWorld[1] + invMat[10] * vNewWorld[2];
	}
	
	/////////////////////////////////////////////////////////////////////////////
	// Transform a point by frame matrix
	public void transformPoint(float vPointSrc[], float vPointDst[])
	{
		float m[] = new float[16];
		getMatrix(m, false); // Rotate and translate
		
		vPointDst[0] = m[0] * vPointSrc[0] + m[4] * vPointSrc[1] + m[8] * vPointSrc[2] + m[12];
		vPointDst[1] = m[1] * vPointSrc[0] + m[5] * vPointSrc[1] + m[9] * vPointSrc[2] + m[13];
		vPointDst[2] = m[2] * vPointSrc[0] + m[6] * vPointSrc[1] + m[10] * vPointSrc[2] + m[14];
	}
	
	////////////////////////////////////////////////////////////////////////////
	// Rotate a vector by frame matrix
	public void rotateVector(float vVectorSrc[], float vVectorDst[])
	{
		float m[] = new float[16];
		getMatrix(m, true); // Rotate only
		
		vVectorDst[0] = m[0] * vVectorSrc[0] + m[4] * vVectorSrc[1] + m[8] * vVectorSrc[2];
		vVectorDst[1] = m[1] * vVectorSrc[0] + m[5] * vVectorSrc[1] + m[9] * vVectorSrc[2];
		vVectorDst[2] = m[2] * vVectorSrc[0] + m[6] * vVectorSrc[1] + m[10] * vVectorSrc[2];
	}
}
